#pragma once

#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <nlohmann/json.hpp>
#include <pcl/features/normal_3d.h>
#include <pcl/point_cloud.h>


using nlohmann::json;


namespace rviz_custom_plugins 
{


namespace models 
{


class PointNormal
{

public:

    PointNormal() = default;

    ~PointNormal() = default;

    int id = 0;

    geometry_msgs::msg::Point position;
    geometry_msgs::msg::Vector3 normal;

    // 强度
    float intensity;
    // 曲率
    float curvature;  


    static pcl::PointXYZINormal toPclPoint(PointNormal pointNormal)
    {
        pcl::PointXYZINormal pclPoint;

        // 设置坐标
        pclPoint.x = pointNormal.position.x;
        pclPoint.y = pointNormal.position.y;
        pclPoint.z = pointNormal.position.z;

        // 设置强度
        pclPoint.intensity = pointNormal.intensity;

        // 设置法线
        pclPoint.normal_x = pointNormal.normal.x;
        pclPoint.normal_y = pointNormal.normal.y;
        pclPoint.normal_z = pointNormal.normal.z;

        // 设置曲率
        pclPoint.curvature = pointNormal.curvature;

        return pclPoint;
    }


    static PointNormal fromPclPoint(pcl::PointXYZINormal pclPoint)
    {
        PointNormal pointNormal;
        pointNormal.position.x = pclPoint.x;
        pointNormal.position.y = pclPoint.y;
        pointNormal.position.z = pclPoint.z;

        pointNormal.normal.x = pclPoint.normal_x;
        pointNormal.normal.y = pclPoint.normal_y;
        pointNormal.normal.z = pclPoint.normal_z;

        pointNormal.intensity = pclPoint.intensity;
        pointNormal.curvature = pclPoint.curvature;

        return pointNormal;
    }

};


inline void to_json(json& j, const PointNormal& point) {

    j = json{
        {"id", point.id},
        {"pos", json::array({point.position.x, point.position.y, point.position.z})},
        {"normal", json::array({point.normal.x, point.normal.y, point.normal.z})},
        {"intensity", point.intensity},
        {"curvature", point.curvature}
    };
}



inline void from_json(const json&j, PointNormal& point)
{
    point.id = j.value("id", 0);

    auto position = j.at("pos");
    point.position.x = position[0];
    point.position.y = position[1];
    point.position.z = position[2];
    
    auto normal = j.at("normal");
    point.normal.x = normal[0];
    point.normal.y = normal[1];
    point.normal.z = normal[2];

    point.intensity = j.value("intensity", 0.0f);
    point.curvature = j.value("curvature", 0.0f);
    
}


}


}


Q_DECLARE_METATYPE(rviz_custom_plugins::models::PointNormal)